 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Motor Shield Example
Description: In this tutorial, we will use a Mbed and a Motor Controller ShieldTutorial Level: BEGINNER
Show EOL distros:
rosserial allows you to easily integrate Mbed-based hardware with ROS. This tutorial will explain how to use a Motor Controller Shield with an Mbed.
You will need an Mbed, a Motor Control Shield, and a way to connect your Sensor to your Mbed such as a breadboard or protoboard.
The Code
Open the Motor Shield example placed on your previously downloaded rosserial/rosserial_mbed/examples directory, and open the MotorShield.cpp file in your favorite text editor:
1 /*
2 * rosserial Motor Shield Example
3 *
4 * This tutorial demonstrates the usage of the
5 * Seeedstudio Motor Shield
6 * http://www.seeedstudio.com/depot/motor-shield-v20-p-1377.html
7 *
8 * Source Code Based on:
9 * https://developer.mbed.org/teams/shields/code/Seeed_Motor_Shield/
10 */
11
12 #include "mbed.h"
13 #include "MotorDriver.h"
14 #include <ros.h>
15 #include <geometry_msgs/Twist.h>
16
17 #ifdef TARGET_LPC1768
18 #define MOTORSHIELD_IN1 p21
19 #define MOTORSHIELD_IN2 p22
20 #define MOTORSHIELD_IN3 p23
21 #define MOTORSHIELD_IN4 p24
22 #define SPEEDPIN_A p25
23 #define SPEEDPIN_B p26
24 #elif defined(TARGET_KL25Z) || defined(TARGET_NUCLEO_F401RE)
25 #define MOTORSHIELD_IN1 D8
26 #define MOTORSHIELD_IN2 D11
27 #define MOTORSHIELD_IN3 D12
28 #define MOTORSHIELD_IN4 D13
29 #define SPEEDPIN_A D9
30 #define SPEEDPIN_B D10
31 #else
32 #error "You need to specify a pin for the sensor"
33 #endif
34
35 MotorDriver motorDriver(MOTORSHIELD_IN1, MOTORSHIELD_IN2, MOTORSHIELD_IN3, MOTORSHIELD_IN4, SPEEDPIN_A, SPEEDPIN_B);
36 ros::NodeHandle nh;
37
38 void messageCb(const geometry_msgs::Twist& msg)
39 {
40 if (msg.angular.z == 0 && msg.linear.x == 0)
41 {
42 motorDriver.stop();
43 }
44 else
45 {
46 if (msg.angular.z < 0)
47 {
48 int speed = (int)(msg.angular.z * -100);
49 motorDriver.setSpeed(speed, MOTORA);
50 motorDriver.setSpeed(speed, MOTORB);
51 motorDriver.goRight();
52 }
53 else if (msg.angular.z > 0)
54 {
55 int speed = (int)(msg.angular.z * 100);
56 motorDriver.setSpeed(speed, MOTORA);
57 motorDriver.setSpeed(speed, MOTORB);
58 motorDriver.goLeft();
59 }
60 else if (msg.linear.x < 0)
61 {
62 int speed = (int)(msg.linear.x * -100);
63 motorDriver.setSpeed(speed, MOTORA);
64 motorDriver.setSpeed(speed, MOTORB);
65 motorDriver.goBackward();
66 }
67 else if (msg.linear.x > 0)
68 {
69 int speed = (int)(msg.linear.x * 100);
70 motorDriver.setSpeed(speed, MOTORA);
71 motorDriver.setSpeed(speed, MOTORB);
72 motorDriver.goForward();
73 }
74 }
75 }
76
77 ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCb);
78 Timer t;
79
80 int main()
81 {
82 t.start();
83 long vel_timer = 0;
84 nh.initNode();
85 nh.subscribe(sub);
86 motorDriver.init();
87 motorDriver.setSpeed(90, MOTORB);
88 motorDriver.setSpeed(90, MOTORA);
89 while (1)
90 {
91 if (t.read_ms() > vel_timer)
92 {
93 motorDriver.stop();
94 vel_timer = t.read_ms() + 500;
95 }
96 nh.spinOnce();
97 wait_ms(1);
98 }
99 }
Compiling and uploading the Code
As seen on the previous tutorial, you must compile the source code by using the makefile in order to generate the .bin file that will be uploaded later to your MBED board. This is no different from uploading any other mbed binary.
Launching the App
Launch roscore
$ roscore
Run the serial node with the right serial port corresponding to your MBED board
$ rosrun rosserial_python serial_node.py _port:=/dev/ttyUSB0
$ rosrun rosserial_python serial_node.py /dev/ttyUSB0
And last, start publishing messages into the cmd_vel topic
$ rostopic pub /cmd_vel geometry_msgs/Twist '[1.0, 0.0, 0.0]' '[0.0, 0.0, 0.0]'
